function total_cost = optimizationFunction(policy_weights, trajectoryLength, target_x, idx, Q, R, use_linear)
    % Get an estimate of the cost of this policy over several trajectories.
    total_cost = 0;

    n = 5;
    
    % Set of five random start states
    states(:, 1) = [0.0065; 0.0016; 0.0094; 0.4001; 0; 0; 0; 0; 0; 0; 0; 23.1901; -10.5383; 14.9343; 0; 0; 0; 0.0350; 0.0286; -0.0016; 0.9990];
    states(:, 2) = [0.0065; 0.0016; 0.0094; 0.4001; 0; 0; 0; 0; 0; 0; 0; -38.0710; 0.3233; 81.8719; 0; 0; 0; 0.2099; 0.2033; -0.4259; -0.8563];
    states(:, 3) = [0.0065; 0.0016; 0.0094; 0.4001; 0; 0; 0; 0; 0; 0; 0; 7.7320; 1.4145; 49.1496; 0; 0; 0; 0.0511; -0.8073; -0.3148; 0.4965];
    states(:, 4) = [0.0065; 0.0016; 0.0094; 0.4001; 0; 0; 0; 0; 0; 0; 0; -28.1345; -2.1514; -67.4943; 0; 0; 0; -0.1659; -0.8382; -0.5184; 0.0347];
    states(:, 5) = [0.0065; 0.0016; 0.0094; 0.4001; 0; 0; 0; 0; 0; 0; 0; 31.1464; 4.2918; 5.7971; 0; 0; 0; -0.5479; -0.0160; 0.5551; -0.6257];
    
    for i=1:n
%         % Determine a start state.
%         start_ned = [20; 10; 50] .* randn(3, 1);
%         rotation_axis = [1; 1; 1] .* randn(3, 1);
%         rotation_axis = rotation_axis / norm(rotation_axis);
%         rotation_angle = 2*pi*randn(1); % radians
%         start_q = [sin(rotation_angle/2)*rotation_axis; cos(rotation_angle/2)];
% 
%         start_state = target_x;
%         start_state(idx.ned) = start_ned;
%         start_state(idx.q) = start_q;

        start_state = states(:, i);
        
        % Generate the joint trajectories for this policy.
        trajectory = rollout(policy_weights, start_state, target_x, trajectoryLength, 1, use_linear);

    
        % Compute the costs for each trajectory.
        cost = TrajectoryCost(trajectory, trajectoryLength, target_x, idx, Q, R);    
        
        % Compute the total cost so far.
        costv(i) = cost;
    end
    
    total_cost = mean(costv);
    
end